import time
import robot_api as rapi
import json

class AGV:
    def __init__(self, label_or_ip, user_name, passwd, https=False):
        if label_or_ip.startswith("puwei_robot_"):
            print("discovery device with label: {}".format(label_or_ip))
            addr = rapi.discovery(label_or_ip)
            if addr == "":
                print("no device found")
            else:
                print("device connect success")
            print("{}: {}".format(label_or_ip, addr))
            rapi.setHost(addr)
        else:
            rapi.setHost(label_or_ip)  # 如果已知设备ip，可以跳过上述设备发现流程
        res = rapi.login(user_name, passwd)
        if not res:
            print( "login failed: " + res.msg)
        else:
            print("login success")
            self.login_status = True
            self.label_or_ip = label_or_ip
            self.map_list  = rapi.listMap().data
            self.status = rapi.status().data
            self.robot_status = rapi.robotStatus().data
            self.set_current_map = None

    def get_status(self):
        self.robot_status = rapi.robotStatus().data
        print(self.robot_status)
        pass

    def load_map(self,map_name):
        rapi.navigation(map_name)

    def set_current_map(self, map_name):

        rapi.navigation(map_name)

    def get_map_list(self):
        pass


    def get_task_list(self):
        pass

    def set_current_task(self):
        pass

    def task_run(self):
        pass
    def task_halt(self):
        pass

    def get_current_pose(self):
        pass

    def get_current_station(self):
        pass

    def move_to_pose(self):
        pass

    def move_to_station_list(self, station_list):
        rapi.moveToStationList(station_list)

    def get_station_list(self, map_name):
        list = rapi.get_station_list(map_name)
        print(list)

    def is_idle(self):
        return True
        self.robot_status = rapi.robotStatus()
        if self.robot_status is not None:
            state = self.robot_status.data['state']
            time.sleep(1)
            return state == 0
        else:
            return False


if __name__ == '__main__':
    with open('task_config.json', 'r+', encoding='utf-8') as f:
        taskConfig = json.load(f)
        print(taskConfig)
    agv = AGV(taskConfig['agv_ip'], taskConfig['user_name'], taskConfig['passwd'])
    agv.task_run()
    while True:
        agv.get_status()
        agv.get_station_list(taskConfig['agv_map_name'])
        station_list = taskConfig['agv_station_list']
        for item in station_list:
            agv.move_to_station_list([item])
            while not agv.is_idle(): pass





